
#include "implement_path_planner/implement_planner.h"
#include "glog_func/log.h"

#include <ros/ros.h>
#include <tf/transform_listener.h>

namespace global_path_planner {
namespace implement {

    ImplementPlanner::ImplementPlanner()
    {
        __interval_Distance = ros::param::param<float>("/implement_planner_path/interval_distance", 0.1);
    }

    ImplementPlanner:: ~ImplementPlanner()
    {

    }

    // 设置记录是否进行
    void ImplementPlanner::SetThreadBusing(bool thread_busing)
    {
        __thread_Busying = thread_busing;
    }

    // 记录手动生成路径
    void ImplementPlanner::RecordManualPath()
    {
        LOG(INFO) << "RecordManualPath start";
        //缓存当前位姿数据
        std::unordered_map<std::string, float> pose_cache;
        while (ros::ok()){
            if(!__thread_Busying){
                LOG(INFO) << "!__thread_Busying exit";
                break;
            }
            ros::Duration(0.01).sleep();
            bool state = false;
            std::unordered_map<std::string, float> pose;
            GetRobotPose(state, pose);
            if(!state){
                continue;
            }
            if(pose_cache.empty()){
                pose_cache = pose;
                continue;
            }
            //计算两点距离和角度
            float angle, distance;
            distance = DistForP2P(pose_cache, pose);
            if(distance > __interval_Distance){
                __way_Points.push_back(pose);
                pose_cache = pose;
            }
        }
        LOG(INFO) << "RecordManualPath finish";
    }

    // 获取手动生成路径
    std::vector<geometry_msgs::Pose> ImplementPlanner::GetImplementPath()
    {
        std::vector<geometry_msgs::Pose> res_path;
        for(auto it = __way_Points.begin(); it != __way_Points.end(); it++){
            geometry_msgs::Pose pose;
            pose.position.x = (*it)["x"];
            pose.position.y = (*it)["y"];
            pose.position.z = 0.;
            //欧拉角转为四元数
            geometry_msgs::Quaternion q;
            q = tf::createQuaternionMsgFromRollPitchYaw(0, 0, (*it)["yaw"]);
            pose.orientation = q;
            res_path.push_back(pose);
        }
        return res_path;
    }

    // geometry_msgs::Pose -> pair<float, float>转换s
    std::vector<std::pair<float, float>> ImplementPlanner::PoseTransferPairs(std::vector<geometry_msgs::Pose>& path_poses){
        std::vector<std::pair<float, float>> path_pairs;
        for(auto pose : path_poses){
            auto path_pair = std::make_pair(pose.position.x, pose.position.y);
            path_pairs.emplace_back(path_pair);
        }
        return path_pairs;
    }

    // 清空缓存数据
    void ImplementPlanner::ClearPath(){
        __way_Points.clear();
    }

    // 获取机器人位姿
    void ImplementPlanner::GetRobotPose(bool& state, std::unordered_map<std::string, float>& pose)
    {
        // 创建一个StampedTransform对象存储变换结果数据
        tf::StampedTransform __transform;
        tf::TransformListener __listener;
        //创建平移和旋转对象
        tf::Vector3 trans;
        tf::Quaternion rot;
        // 四元数转化为欧拉角
        double roll, pitch, yaw;
        for(int i = 0; i < 10; i++){
            try{
                // 向侦听器查询特定的转换，(/base_link到/map的变换)，转换的时间ros::Time(0)提供了最新的可用转换。
                __listener.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(3.0));
                __listener.lookupTransform("map", "base_link", ros::Time(0), __transform);
                trans = __transform.getOrigin();
                rot = __transform.getRotation();
                tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);
                pose["x"] = trans[0];
                pose["y"] = trans[1];
                pose["yaw"] = yaw;
                state = true;
            }catch(tf2::TransformException &ex){
                std::string msg = "[ImplementPlanner::GetRobotPose]" + std::string(ex.what());
            }
            // 查询到退出循环
            if(state){
                break;
            }
        }
        if(!state){
            pose["x"] = 0.0;
            pose["y"] = 0.0;
            pose["yaw"] = 0.0;
            state = false;
        }        
    }

    // 计算两点之间的距离
    float ImplementPlanner::DistForP2P(std::unordered_map<std::string, float>& pre,
                                    std::unordered_map<std::string, float>& cur)
    {
        float dx = pre["x"] - cur["x"];
        float dy = pre["y"] - cur["y"];
        float distance = hypot(dx,dy);
        return distance;
    }


}  // namespace implement
}  // namespace global_path_planner